#include <fcntl.h>
#include <stdio.h>
#include <ivs_base_move_type.h>
#ifdef DMALLOC
#include <imp/dmalloc.h>
#endif

static void sadSum(IMat src, IMat dst, int kernel_w, int kernel_h)
{
	int i, j, si, sj, sum;
	int di, dj;

	for(i = 0, di = 0; i <= src.rows-kernel_h; i+=kernel_h, di++)
    {
		//printf("dst: row:%d, %d, %d\n",i, src.rows-kernel_h, di);
		for(j = 0, dj = 0; j <= src.cols-kernel_w; j+=kernel_w, dj++)
		{
			sum = 0;
			for(si = 0; si < kernel_h; si++)
			{
				for(sj = 0; sj < kernel_w; sj++)
				{
					sum += *(src.data +(i+si)*src.step +(j+sj));
				}
			}
			*(dst.data +di*dst.step +dj) = (uint8_t)sum;
		}
    }
}

static void JzMergeSad(IMat src1, IMat src2, IMat dst, int thresh, int kernel_w, int kernel_h){//absdiff+thresh+erode+sum
	int i, j, si, sj, sum;
	int di, dj;
	int ci, cj;
	int l1_step, l2_step, l3_step;
	int row_1, row_2, row_3;
	int col_1, col_2, col_3;
	uint8_t min_value;
	uint8_t tmp;
	int i_limit = src1.rows-1;
	int j_limit = src1.cols-1;
	int total_count = 0;
	for(i = 0, di = 0; i <= src1.rows-kernel_h; i+=kernel_h, di++)
    {
		//printf("dst: row:%d, %d, %d\n",i, src.rows-kernel_h, di);
		for(j = 0, dj = 0; j <= src1.cols-kernel_w; j+=kernel_w, dj++)
		{
			sum = 0;
			for(si = 0; si < kernel_h; si++)
			{
				for(sj = 0; sj < kernel_w; sj++)
				{
					ci = (i+si);
					cj = (j+sj);
					row_1 = (ci-1)<0?0:ci-1;
					row_2 = ci;
					row_3 = (ci+1)>i_limit?i_limit:ci+1;
					col_1 = (cj-1)<0?0:cj-1;
					col_2 = cj;
					col_3 = (cj+1)>j_limit?j_limit:cj+1;
					l1_step = row_1*src1.step;
					l2_step = row_2*src1.step;
					l3_step = row_3*src1.step;

					/*0*/
					min_value = 255;
					tmp = abs( *(src1.data+l1_step+col_1) - *(src2.data+l1_step+col_1) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
					/*1*/
					tmp = abs( *(src1.data+l1_step+col_2) - *(src2.data+l1_step+col_2) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
					/*2*/
					tmp = abs( *(src1.data+l1_step+col_3) - *(src2.data+l1_step+col_3) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
                    /*3*/
					tmp = abs( *(src1.data+l2_step+col_1) - *(src2.data+l2_step+col_1) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
					/*4*/
					tmp = abs( *(src1.data+l2_step+col_2) - *(src2.data+l2_step+col_2) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
					/*5*/
					tmp = abs( *(src1.data+l2_step+col_3) - *(src2.data+l2_step+col_3) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
					/*6*/
					tmp = abs( *(src1.data+l3_step+col_1) - *(src2.data+l3_step+col_1) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
					/*7*/
					tmp = abs( *(src1.data+l3_step+col_2) - *(src2.data+l3_step+col_2) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;
					/*8*/
					tmp = abs( *(src1.data+l3_step+col_3) - *(src2.data+l3_step+col_3) );
					tmp = (tmp < thresh) ? 0:tmp;
					min_value = min_value < tmp ? min_value : tmp;

					sum += min_value;
				}
			}
			*(dst.data +di*dst.step +dj) = (uint8_t)sum;
			if((uint8_t)sum>0)
				total_count++;
		}
    }
//	printf("total_count:  %d\n", total_count);
}

int sad(IMat src1, IMat src2, IMat temp, IMat dst, int Mode, int thresh, int isUseSimd)
{
	if((src1.data==NULL)||(src2.data==NULL)||(temp.data==NULL)||(dst.data==NULL))
	{
		printf("%s: %d: invalid parameter error: input data pointer is NULL: %p ,%p, %p\n",__func__,__LINE__,src1.data, src2.data, dst.data);
		return -1;
	}
	if((src1.cols != src2.cols)||(src1.rows != src2.rows))
	{
		printf("%s: %d: invalid parameter error: Size of src1 is different from Size of src2\n",__func__,__LINE__);
		return -1;
	}

	int kernel_w, kernel_h;
	int i, j, si, sj, sum;
	uint8_t tmp;
	int di, dj;
	switch(Mode)
	{
	case 0:{//8*8
		kernel_w = 8;
		kernel_h = 8;
		if((src1.cols%kernel_w != 0)||(src1.rows%kernel_h !=0))
		{
			printf("%s: %d: invalid parameter error: src.width MOD %d or src.height MOD %d is not equal to 0\n",__func__,__LINE__,kernel_w,kernel_h);
			return -1;
		}
		if(isUseSimd){
			MergeBaseMove(src1, src2, temp, thresh, isUseSimd);
			sadSum(temp, dst, kernel_w, kernel_h);
		}else{
			JzMergeSad(src1, src2, dst, thresh, kernel_w, kernel_h);
		}
	}
		break;
	default:
		printf("%s: %d: invalid parameter error:Mode out of range\n",__func__,__LINE__);
		break;
	}
	return 0;
}
